// Exemplos de como Utilizar Listas Simples Encadeadas em C++

#include <listas.h>
#include <cstdlib>
#include <cstdio>

void Inicializa_Lista_Mapa(list<lista_mapa> *Lista1)
{
  // Variaveis
  lista_mapa aux_mapa;
  list<lista_mapa>::iterator im;
  int i;

  // Inicializando a Lista Mapa

  aux_mapa.media_x = 0;
  aux_mapa.media_y = 0;
  aux_mapa.flag_land = 0;

  for(i = 0; i<12; i++)
  {
    aux_mapa.color = i+1;
    Lista1->push_back(aux_mapa);      
  }   
                             
}

void Inicializa_Lista_Sensor(list<lista_sensor> *Lista2)
{
  // Variaveis
  lista_sensor aux_sensor;
  list<lista_sensor>::iterator is;
  int i;

  // Inicializando a Lista 02
  aux_sensor.dist = 0;
  aux_sensor.theta = 0;
  aux_sensor.flag = 0;

  for(i = 0; i<12; i++)
  {
    aux_sensor.color = i+1;
    Lista2->push_back(aux_sensor);
  }

}

void Atualiza_Lista_Mapa(list<lista_mapa> *Lista1, PGMATRIX Matriz_Mapa)
{
  // Variavel
  list<lista_mapa>::iterator im;
  int i,j;

  im = Lista1->begin();
  for (i = 0; i<12; i++)
  {
    j = 3*(i+1) + 1;
    im->media_x = im->media_x + PGMATRIX_DATA(Matriz_Mapa, j,1);
    im->media_y = im->media_y + PGMATRIX_DATA(Matriz_Mapa, j+1,1);
    
    //std::cout << "Matriz_Mapa[" << j << "]= " << PGMATRIX_DATA(Matriz_Mapa, j,1) << std::endl;
    //std::cout << "Matriz_Mapa[" << j+1 << "]= " << PGMATRIX_DATA(Matriz_Mapa, j+1,1) << std::endl;
    std::cout << "landmark " << i << ": (" << im->media_x << "," << im->media_y << ")\n";

    ++im;
  }
  std::cout << std::endl;
   
}

void Atualiza_Lista_Sensor(list<lista_sensor> *Lista2, double dist_new[], double theta_new[], int signature[])
{
  // Variaveis
  list<lista_sensor>::iterator is;
  int i;

  is = Lista2->begin();
  for (i = 0; i<12; i++)
  {
    if (signature[i] == 1)
    {
      is->dist = dist_new[i];
      is->theta = theta_new[i];
      is->flag = 1;
    }
    
    if (signature[i] == 0)
    {
      is->flag = 0;
    }

    ++is;
  }
}

int Ler_Lista_Mapa(list<lista_mapa> *Lista1, int sig_flag, double pos_landmark[])
{
  // Variaveis
  list<lista_mapa>::iterator im;
  int i;
 
  im = Lista1->begin();
  for(i = 0; i<sig_flag; i++)
    ++im;
  
  // Recebendo os valores
  pos_landmark[0] = im->media_x;		// Recebe a coordenada X do Landmark em relação ao Referencial Global
  pos_landmark[1] = im->media_y;		// Recebe a coordenada Y do Landmark em relação ao Referencial Global
  pos_landmark[2] = im->flag_land;		// Recebe o flag_land
  
  return sig_flag;
  
}

void Ler_Lista_Sensor(list<lista_sensor> *Lista2, int sig_flag, double z_landmark[])
{
  // Variaveis
  list<lista_sensor>::iterator is;
  int i;
 
  // IS WE!
  is = Lista2->begin();
  for(i = 0; i<sig_flag; i++)
    ++is;
   
  // Recebendo os valores
  z_landmark[0] = is->dist;		// Recebe a distancia linear do Landmark em relação ao Referencial do Robo fornecido pelo Sensor
  z_landmark[1] = is->theta;		// Recebe a distancia angular do Landmark em relação ao Referencial do Robo fornecido pelo Sensor
  z_landmark[2] = 0;
}

void Atualiza_Flag_Land(list<lista_mapa> *Lista1, int sig_flag, double media_landmark[])
{
  // Variaveis
  list<lista_mapa>::iterator im;
  int i;
 
  im = Lista1->begin();
  for(i = 0; i<sig_flag; i++)
    ++im;
   
  // Recebendo os valores
  im->media_x = media_landmark[0]; 
  im->media_y = media_landmark[1];
  im->flag_land = 1;
}

